/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "air_service/modules/perception-camera/pipeline/camera_perception_component.h"

#include "air_middleware_rate.h"
#include "base/common/log.h"
#include "base/common/time_util.h"
#include "base/io/file_util.h"
#ifdef PERCEPTION_VIZ
#include "air_service/modules/perception-visualization/pipline/camera_perception_viz_message.h"
using airos::perception::visualization::CameraPerceptionVizMessage;
#endif
namespace airos {
namespace perception {
namespace camera {

inline airos::base::device::CameraInitConfig GetCameraParam(
    const PerceptionParam& perception_param) {  // TODO
  airos::base::device::CameraInitConfig ret;
  ret.camera_type = perception_param.camera_param().type();
  ret.camera_name = perception_param.camera_param().name();
  ret.user = perception_param.camera_param().user();
  ret.password = perception_param.camera_param().password();
  ret.ip = perception_param.camera_param().ip();
  ret.channel_num = perception_param.camera_param().channel();
  ret.stream_num = perception_param.camera_param().stream();
  ret.img_mode = airos::base::Color::BGR;
  ret.gpu_id = perception_param.gpu_id();
  return ret;
}

inline ObstacleCameraPerceptionParam GetObstacleCameraPerceptionParam(
    const PerceptionParam& perception_param) {
  ObstacleCameraPerceptionParam ret;
  for (int i = 0; i < perception_param.plugin_param_size(); ++i) {
    airos::base::PluginParam plugin_param;
    auto& proto_param = perception_param.plugin_param(i);
    plugin_param.name = proto_param.name();
    plugin_param.config_file = airos::base::FileUtil::GetAbsolutePath(
        proto_param.root_dir(), proto_param.config_file());
    plugin_param.extra_describe = perception_param.camera_param().name();
    ret.plugin_params.emplace_back(std::move(plugin_param));
  }
  return ret;
}

CAMERA_DETECTION_COMPONENT::~CAMERA_DETECTION_COMPONENT() {
  running_.exchange(false);
  if (proc_thread_ != nullptr && proc_thread_->joinable()) {
    proc_thread_->join();
  }
}

bool CAMERA_DETECTION_COMPONENT::InitConfig() {
  CHECK(LoadConfig<PerceptionParam>(&perception_param_))
      << "Read config failed: ";
  return true;
}

bool CAMERA_DETECTION_COMPONENT::InitCameraDriver() {
  camera_.reset(new airos::middleware::device_service::CameraService());
  CHECK(camera_->Init(GetCameraParam(perception_param_)))
      << "Init Camera failed ";
  return true;
}

bool CAMERA_DETECTION_COMPONENT::InitAlgorithmPipeline() {
  camera_obstacle_pipeline_.reset(new ObstacleCameraPerception());
  CHECK(camera_obstacle_pipeline_ != nullptr);
  CHECK(camera_obstacle_pipeline_->Init(
            GetObstacleCameraPerceptionParam(perception_param_)) != false)
      << "init camera obstacle pipeline failed";
  return true;
}

bool CAMERA_DETECTION_COMPONENT::Init() {
  if (InitConfig() == false) {
    LOG_ERROR << "InitConfig() failed.";
    return false;
  }

  if (InitCameraDriver() == false) {
    LOG_ERROR << "InitCameraDriver failed.";
    return false;
  }

  if (InitAlgorithmPipeline() == false) {
    LOG_ERROR << "InitAlgorithmPipeline failed.";
    return false;
  }

  running_.store(true);
  proc_thread_.reset(new std::thread(
      std::bind(&CAMERA_DETECTION_COMPONENT::ProcThread, this)));
  return true;
}

void CAMERA_DETECTION_COMPONENT::ProcThread() {
  airos::middleware::AirMiddlewareRate perception_rate(
      perception_param_.frequency());
  while (running_.load()) {
    double start_time = airos::base::TimeUtil::GetCurrentTime();
    PerceptionFrame camera_frame;
    auto camera_data = camera_->GetCameraData();
    if (camera_data == nullptr) {
      LOG_ERROR << "camera get empty.";
      continue;
    }
    double get_img_time = airos::base::TimeUtil::GetCurrentTime();
    LOG_INFO << "perception get image cost "
             << static_cast<int>((get_img_time - start_time) * 1000) << " ms";
    camera_frame.image = camera_data->image;
    camera_frame.camera_name = camera_data->camera_name;
    camera_frame.timestamp = camera_data->timestamp * 1e-6;
    camera_frame.frame_id = camera_data->sequence_num;  // TODO

    if (camera_obstacle_pipeline_->Perception(camera_frame) == false) {
      LOG_ERROR << "Perception fail.";
      continue;
    }
    double perception_time = airos::base::TimeUtil::GetCurrentTime();
    LOG_INFO << "perception cost "
             << static_cast<int>((perception_time - get_img_time) * 1000)
             << " ms";

    // TODO
    std::shared_ptr<PerceptionObstacles> out_message(new PerceptionObstacles());
    SerializeMsg(camera_frame, *out_message);
    std::string output_obstacles_channel_name_ =
        "/v2x/perception/obstacles_" + camera_data->camera_name;
    // LOG_INFO << "output_obstacles_channel_name_: " <<
    // output_obstacles_channel_name_;

    if (Send(output_obstacles_channel_name_, out_message) == false) {
      LOG_ERROR << "send_perception_obstacles_msg failed. channel_name: "
                << output_obstacles_channel_name_;
    }
#ifdef PERCEPTION_VIZ
    std::shared_ptr<CameraPerceptionVizMessage> viz_msg(
        new CameraPerceptionVizMessage(
            camera_frame.camera_name, camera_frame.timestamp,
            camera_frame.camera2world_pose.matrix(),
            camera_frame.camera_k_matrix, camera_frame.image->blob(),
            camera_frame.objects));
    Send("v2x/perception/viz", viz_msg);
#endif
    perception_rate.Sleep();
  }
}

void CAMERA_DETECTION_COMPONENT::SerializeMsg(PerceptionFrame& frame,
                                              PerceptionObstacles& out_msg) {
  auto header = out_msg.mutable_header();
  double current_time = airos::base::TimeUtil::GetCurrentTime();
  header->set_timestamp_sec(current_time);              // second
  header->set_frame_id(frame.camera_name);              // TODO
  header->set_camera_timestamp(frame.timestamp * 1e9);  // nano-second
  // // frame.camera_name;
  for (auto& object : frame.objects) {
    auto obstacle = out_msg.add_perception_obstacle();
    obstacle->set_timestamp(frame.timestamp);
    obstacle->set_id(object.track_id);

    // frame.type_id_confidence
    auto box = obstacle->mutable_bbox2d();
    box->set_xmin(object.box.left_top.x);
    box->set_ymin(object.box.left_top.y);
    box->set_xmax(object.box.right_bottom.x);
    box->set_ymax(object.box.right_bottom.y);

    obstacle->set_length(object.size.length);
    obstacle->set_width(object.size.width);
    obstacle->set_height(object.size.height);
    obstacle->set_theta(object.theta);

    auto position = obstacle->mutable_position();
    position->set_x(object.center(0));
    position->set_y(object.center(1));
    position->set_z(object.center(2));

    switch (object.is_occluded) {
      case algorithm::TriStatus::UNKNOWN:
        obstacle->set_occ_state(OcclusionState::OCC_UNKNOWN);
        break;
      case algorithm::TriStatus::TRUE:
        obstacle->set_occ_state(OcclusionState::OCC_COMPLETE_OCCLUDED);
        break;
      case algorithm::TriStatus::FALSE:
        obstacle->set_occ_state(OcclusionState::OCC_NONE_OCCLUDED);
        break;
      default:
        break;
    }

    switch (object.is_truncated) {
      case algorithm::TriStatus::UNKNOWN:
        obstacle->set_trunc_state(TruncationState::TRUNC_UNKNOWN);
        break;
      case algorithm::TriStatus::TRUE:
        obstacle->set_trunc_state(TruncationState::TRUNC_TRUE);
        break;
      case algorithm::TriStatus::FALSE:
        obstacle->set_trunc_state(TruncationState::TRUNC_FALSE);
        break;
      default:
        break;
    }

    obstacle->set_theta_variance(object.theta_variance);

    for (auto& pt : object.pts8.pts8) {
      auto point = obstacle->add_cube_pts8();
      point->set_x(pt.x);
      point->set_y(pt.y);
    }

    obstacle->set_sub_type(static_cast<perception::SubType>(object.type));
    for (int i = 0; i < object.sub_type_probs.size(); ++i) {
      auto sub_type_prob = obstacle->add_sub_type_probs();
      auto sub_type = static_cast<perception::SubType>(i);
      sub_type_prob->set_sub_type(sub_type);
      sub_type_prob->set_prob(object.sub_type_probs[i]);
    }
    obstacle->set_sub_type_id_confidence(object.type_id_confidence);
    obstacle->set_sub_type_id(object.type_id);

    // PerceptionObstacle 没有对应字段
    // algorithm::Point2f bottom_uv; // 1点
    // float alpha = 0;           //朝向
    // float direction_alpha = 0; //朝向360
    // algorithm::TriStatus is_truncated = algorithm::TriStatus::UNKNOWN;
    // //是否截断（具体见标注文档） int caseflag = 0;
    // //属于1个点不可见还是2个点不可见的case algorithm::Point3f direction ;
    // //依赖 theta计算出 Eigen::Vector3f local_center;
  }
}

}  // namespace camera
}  // namespace perception
}  // namespace airos